 #include <Arduino.h>
#include <SPI.h>
#include <Wire.h>
#include <BleGamepad.h>
#include <RF24.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

//#include <Preferences.h>

#include <LinkedList.h>
#include <EEPROM.h>

#include "Freenove_WS2812_Lib_for_ESP32.h"

#include "MPU6050_tockn.h"
#include "ArduinoGL.h"

#define CE 22
#define CSN 5
RF24 radio(CE, CSN);
const uint64_t radio_pipe = 0xF0F0F0F0E1LL;


#define CONFIG_IDX_RGB      0
#define CONFIG_IDX_PA_LEVEL 1

#define CONFIG_FLAG 168

int PA_LEVELS[]= {RF24_PA_MIN,RF24_PA_LOW,RF24_PA_HIGH,RF24_PA_MAX};
char *ConfigName[] = {"RGB","PA_LEVEL"};
byte ConfigValue[] = {1,0};
byte ConfigValueMin[]  = {0,0};
byte ConfigValueStep[] = {1,1};
byte ConfigValueMax[]  = {9,3};
bool ConfigValueType[] ={0,1};

char *ConfigRGBValueName[]={"NONE","RED","YELLOW","GREEN","CYAN","BLUE","PURPLE","PINK","YELLOW2","GREEN2"};
char *ConfigPALevelName[]={"MIN","LOW","HIGH","MAX"};

int  MenuSelectIndex = 0;

#define STATE_CONTROL  0 
#define STATE_CONFIG   1

int  State = STATE_CONTROL;


bool DEBUG_INFO = false;
int  LoopCounter = 0;

//RGB COLOR 
int color_table[10][3]={
  {0,0,0},  
  {255,0,0},
  {255,130,0},
  {84,255,0},
  {0,253,214},
  {0,0,255},
  {222,0,255},
  {255,0,156},  
  {255,255,0},  
  {0,255,0}    
};

#define LEDS_COUNT  4
#define LEDS_PIN  14
#define CHANNEL   0
Freenove_ESP32_WS2812 strip = Freenove_ESP32_WS2812(LEDS_COUNT, LEDS_PIN, CHANNEL,TYPE_GRB);


//BLE GAMEPAD
#define numOfButtons 4
#define numOfHatSwitches 0
#define enableX true
#define enableY true
#define enableZ true
#define enableRX true
#define enableRY true
#define enableRZ true
#define enableSlider1 false
#define enableSlider2 false
#define enableRudder false
#define enableThrottle false
#define enableAccelerator false
#define enableBrake false
#define enableSteering false

BleGamepad bleGamepad("Shanzai Gyro Contoller", "ESP32Device", 100);

struct RawPackage {  
  byte j1PotX; 
  byte j1PotY;
  byte pot1;
  byte button1;
  byte button2;
  byte button3;
  byte button4;
  byte yaw;
  byte pitch;
  byte roll; 
};

RawPackage   data,last_data;

int j1PotXCenter = 2048;
int j1PotXRange = 2048;

int j1PotYCenter = 2048;
int j1PotYRange = 2048;

int PotCenter = 1600;
int PotRange = 800;

bool bButton1Down = false;
bool bButton2Down = false;
bool bButton3Down = false;
bool bButton4Down = false;

#define I2C_Freq 200000
#define SDA_0 17
#define SCL_0 16
TwoWire I2C_0 = TwoWire(0);

#define SCREEN_WIDTH  128
#define SCREEN_HEIGHT 64

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &I2C_0);

MPU6050 mpu6050(I2C_0);   

#define J1X_PIN  35
#define J1Y_PIN  34
#define POT_PIN  4

#define BTN1_PIN 33
#define BTN2_PIN 25
#define BTN3_PIN 26
#define BTN4_PIN 27

void setup() 
{
  Serial.begin(115200);

  InitEEPROM();
    
  I2C_0.begin(SDA_0 , SCL_0 , I2C_Freq);  

  Serial.println("Display Init!");
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3c))
  {
    Serial.println(F("SSD1306 allocation failed"));
  }
  else
  {
    display.clearDisplay();
    display.display();
  } 

//CONFIGURE
   pinMode(BTN1_PIN,INPUT_PULLUP);
   pinMode(BTN2_PIN,INPUT_PULLUP);
   pinMode(BTN3_PIN,INPUT_PULLUP);
   pinMode(BTN4_PIN,INPUT_PULLUP); 
   pinMode(LEDS_PIN,OUTPUT); 
   analogReadResolution(12);  
   adcAttachPin(J1X_PIN);
   adcAttachPin(J1Y_PIN);
   adcAttachPin(POT_PIN);
   

  Serial.println("LED Init!");
  strip.begin();
  UpdateStripColor();

  Serial.println("Blue Init!");
  BleGamepadConfiguration bleGamepadConfig;
  bleGamepadConfig.setAutoReport(false);
  bleGamepadConfig.setControllerType(CONTROLLER_TYPE_MULTI_AXIS); // CONTROLLER_TYPE_JOYSTICK, CONTROLLER_TYPE_GAMEPAD (DEFAULT), CONTROLLER_TYPE_MULTI_AXIS
  bleGamepadConfig.setButtonCount(numOfButtons);
  bleGamepadConfig.setWhichAxes(enableX, enableY, enableZ, enableRX, enableRY, enableRZ, enableSlider1, enableSlider2);      // Can also be done per-axis individually. All are true by default
  bleGamepadConfig.setWhichSimulationControls(enableRudder, enableThrottle, enableAccelerator, enableBrake, enableSteering); // Can also be done per-control individually. All are false by default
  bleGamepadConfig.setHatSwitchCount(numOfHatSwitches);                                                                      // 1 by default
  // Some non-Windows operating systems and web based gamepad testers don't like min axis set below 0, so 0 is set by default
  bleGamepadConfig.setAxesMin(0x8001); // -32767 --> int16_t - 16 bit signed integer - Can be in decimal or hexadecimal
  bleGamepadConfig.setAxesMax(0x7FFF); // 32767 --> int16_t - 16 bit signed integer - Can be in decimal or hexadecimal 
  
  bleGamepad.begin(&bleGamepadConfig);

  // Define the radio communication
  Serial.println("NRF24L01 Init!");
  radio.begin();
  radio.setChannel(115);
  radio.openWritingPipe(radio_pipe);
  radio.setAutoAck(false);
  radio.setDataRate(RF24_250KBPS);
  radio.setPALevel(PA_LEVELS[0]);
  radio.stopListening();
 
  Serial.println("MPU6050 Init!");
  mpu6050.begin();     
  mpu6050.calcGyroOffsets(true);

//  glUseCanvas(&u8g2);
//  glClear(GL_COLOR_BUFFER_BIT);
//    
//  glMatrixMode(GL_PROJECTION);
//  glLoadIdentity();
//  gluPerspective(30.0, u8g2.getDisplayWidth()/u8g2.getDisplayHeight(), 0.1f, 9999.f);
//    
//  glMatrixMode(GL_MODELVIEW);

  AutoCalibrate();
  
  delay(100);

  UpdateStripColor();
}

void loop() 
{
   mpu6050.update();     
   
   
   LoopCounter = (LoopCounter+5) % 1000;  
   int j1PotX = max(min(analogRead(J1X_PIN),j1PotXCenter+j1PotXRange),j1PotXCenter-j1PotXRange);     
   data.j1PotX = 100 - map(j1PotX,j1PotXCenter-j1PotXRange, j1PotXCenter+j1PotXRange, 0, 100);  
   
   int j1PotY = max(min(analogRead(J1Y_PIN),j1PotYCenter+j1PotYRange),j1PotYCenter-j1PotYRange);    
   data.j1PotY = 100 - map(j1PotY,j1PotYCenter-j1PotYRange, j1PotYCenter+j1PotYRange, 0, 100);    

   int Pot = max(min(analogRead(POT_PIN),PotCenter+PotRange),PotCenter-PotRange);    
   data.pot1 = 100 - map(Pot,PotCenter-PotRange,PotCenter+PotRange,0,100);

   Serial.println(analogRead(POT_PIN));      

   data.button1 = 1 - digitalRead(BTN1_PIN);
   data.button2 = 1 - digitalRead(BTN2_PIN);
   data.button3 = 1 - digitalRead(BTN3_PIN);
   data.button4 = 1 - digitalRead(BTN4_PIN);

   data.yaw = map(constrain(mpu6050.getAngleZ(), -90, +90), -90, +90, 0, 255);   //对角度进行简单的转换
   data.roll = map(constrain(mpu6050.getAngleX(), -90, +90), -90, +90, 0, 255);   //对角度进行简单的转换
   data.pitch = map(constrain(mpu6050.getAngleY(), -90, +90), -90, +90, 255, 0);
   
   switch(State) 
   {
    case STATE_CONTROL:
      HandleControlInput();      
      ShowControl();
      break;
    case STATE_CONFIG:
      HandleConfigInput();
      ShowConfig();
      break;
   }

   if (radio.isChipConnected())
   {
      radio.write(&data, sizeof(RawPackage));
   }
  
   

 //glClear(GL_COLOR_BUFFER_BIT);   
 // glLoadIdentity();
 // gluLookAt(-10, 10, 0, 0, 0, 0, 0, 1, 0);    
 // glRotatef(YAW, 0.f, 1.f, 0.f);
 // glRotatef(PITCH, 1.f, 0.f, 0.f);
 // glRotatef(ROLL, 0.f, 0.f, 1.f);
 // glScalef(2.5f, 2.5f, 2.5f);
 //
  if (bleGamepad.isConnected())
  {
        Serial.println("Press buttons 5, 16 and start. Move all enabled axes to max. Set DPAD (hat 1) to down right.");
        if (data.button1)
        {
          bleGamepad.press(BUTTON_1);  
          bleGamepad.sendReport();
        }
        else
        {
          bleGamepad.release(BUTTON_1);  
          bleGamepad.sendReport();
        }
        if (data.button2)
        {
          bleGamepad.press(BUTTON_2);
          bleGamepad.sendReport();
        }
        else
        {
          bleGamepad.release(BUTTON_2);  
          bleGamepad.sendReport();
        }
        if (data.button3)
        {
          bleGamepad.press(BUTTON_3);
          bleGamepad.sendReport();
        }
         else
        {
          bleGamepad.release(BUTTON_3);  
          bleGamepad.sendReport();
        }       
        
        int XA = - map(data.j1PotX,0,100,-32760,32760);
        bleGamepad.setX(XA);    
        bleGamepad.sendReport();    
        
        int YA = - map(data.j1PotY,0,100,-32760,32760);
        bleGamepad.setY(YA);
        bleGamepad.sendReport();

         int ZA =  map(constrain(data.pot1,0,100),0,100,-32760,32760);
        bleGamepad.setZ(ZA);
        bleGamepad.sendReport();   

        float Roll = constrain(mpu6050.getAngleX(),-90,90);
        float Pitch= constrain(mpu6050.getAngleY(),-90,90);
        //float Yaw = 0;//constrain(mpu6050.getAngleZ(),-90,90);

        int RXA = map(Roll,-90,90,-32760,32760); 
        bleGamepad.setRX(RXA);   
        bleGamepad.sendReport();    

        int RYA = map(Pitch,-90,90,-32760,32760); 
        bleGamepad.setRY(RYA);   
        bleGamepad.sendReport();    

        //int RZA = map(Yaw,-90,90,-32760,32760); 
        //bleGamepad.setRZ(RZA);   
        //bleGamepad.sendReport();    
    
       //  Serial.print(analogRead(3));
       //  Serial.print(" ");
       //  Serial.print(T);
       //  Serial.print(" ");
       //  Serial.print(Yaw);
       //  Serial.println(" ");
  }  
  delay(1);  
}

//void drawAxis()
//{                
//  glBegin(GL_POLYGON);
//    glVertex3f(-2, 0, 0);
//    glVertex3f(3, 0, 0);
//  glEnd();  
//  glBegin(GL_POLYGON);
//  glVertex3f(0, -1, 0);
//    glVertex3f(0, 1, 0);
//  glEnd();  
//  glBegin(GL_POLYGON);
//      glVertex3f(0, 0, 0);
//    glVertex3f(0, 0,0.5);
//  glEnd();      
//}


//void drawCube() {                
//
//  glBegin(GL_POLYGON);
//    glVertex3f(-1, -1, -1);
//    glVertex3f(1, -1, -1);
//    glVertex3f(1, 1, -1);
//    glVertex3f(-1, 1, -1);
//  glEnd();
//
//  glBegin(GL_POLYGON);
//    glVertex3f(1, -1, -1);
//    glVertex3f(1, -1, 1);
//    glVertex3f(1, 1, 1);
//    glVertex3f(1, 1, -1);
//  glEnd();
//
//  glBegin(GL_POLYGON);
//    glVertex3f(1, -1, 1);
//    glVertex3f(-1, -1, 1);
//    glVertex3f(-1, 1, 1);
//    glVertex3f(1, 1, 1);
//  glEnd();
//
//  glBegin(GL_POLYGON);
//    glVertex3f(-1, -1, 1);
//    glVertex3f(-1, -1, -1);
//    glVertex3f(-1, 1, -1);
//    glVertex3f(-1, 1, 1);
//  glEnd();
//
//  glBegin(GL_POLYGON);
//    glVertex3f(-1, -1, 1);
//    glVertex3f(1, -1, 1);
//    glVertex3f(1, -1, -1);
//    glVertex3f(-1, -1, -1);
//  glEnd();
//
//  glBegin(GL_POLYGON);
//    glVertex3f(-1, 1, -1);
//    glVertex3f(1, 1, -1);
//    glVertex3f(1, 1, 1);
//    glVertex3f(-1, 1, 1);
//  glEnd();
//}

char buffer[128];
void ShowControl()
{      
   display.clearDisplay();
  if (DEBUG_INFO)
  {
     display.setTextColor(WHITE);
  
     sprintf(buffer," [J] %3.3d %3.3d %3.3d %3.3d",
     data.j1PotX,data.j1PotY,0,0);
     display.setCursor(0,0);
     display.print(buffer);

     sprintf(buffer," [P] %3.3d %3.3d %3.3d %3.3d",
     data.pot1,0,0,0,0,0);
     display.setCursor(0,14);
     display.print(buffer);
       
     sprintf(buffer," [G] %3.3d %3.3d %3.3d %3.3d",
     data.yaw,data.pitch,data.roll,0);    
     display.setCursor(0,28);
     display.print(buffer);
       
     sprintf(buffer," [B] %2.2d %2.2d %2.2d %2.2d",
     data.button1,data.button2,data.button3,data.button4);
     display.setCursor(0,42);
     display.print(buffer);    

     sprintf(buffer," [C] %d %d",
     j1PotXCenter,j1PotXRange);
     display.setCursor(0,42+14);
     display.print(buffer);    
  }
  else
  {
      for (int j = 0; j < 4 ;j++)
      {
        for (int i = 0; i < 6; i++)
         {
           display.drawPixel(i*25.5, j*20,SSD1306_WHITE);
         }
       }
        
        int w = 22 + ((LoopCounter % 10) / 10.0f)* 2;
        display.drawRect(64-w,32 + 6 -w,w*2,w*2,SSD1306_WHITE);               
        //display.setDrawColor(1);
        display.drawCircle(64 + (1.0f - data.j1PotX/100.0f) * 28 - 14,32+6+(1.0f - data.j1PotY/100.0f) * 28 - 14,4,SSD1306_WHITE);
    
        display.drawRect((128-42)/2,0,42,6,SSD1306_WHITE);        
        display.fillRect((128-42)/2 + 42 * (1.0f-data.pot1 / 100.0f),0,42 * (data.pot1 / 100.0f)+1,6,SSD1306_WHITE);

        float Roll = constrain(mpu6050.getAngleX(),-90,90);
        float Pitch= constrain(mpu6050.getAngleY(),-90,90);
        float Yaw= constrain(mpu6050.getAngleZ(),-90,90);

        display.drawLine(64-Roll/2,0,64-Roll/2,64,SSD1306_WHITE);
        display.drawLine(0,32+Pitch/2,128,32+Pitch/2,SSD1306_WHITE);
       
        (data.button1 == 1)? display.fillRect(0,46,10,8,SSD1306_WHITE): display.drawRect(0,46,10,8,SSD1306_WHITE);
        (data.button2 == 1)? display.fillRect(0,18,10,8,SSD1306_WHITE): display.drawRect(0,18,10,8,SSD1306_WHITE);          
        (data.button3 == 1)? display.fillRect(118,38,10,8,SSD1306_WHITE): display.drawRect(118,38,10,8,SSD1306_WHITE);         
    }        
    display.display();
    delay(1);
}   

void ShowConfig()
{      
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  int StartIndex = ((MenuSelectIndex+1) - 5);
  if (StartIndex < 0)
  {
    StartIndex = 0;
  }
  for(int i = 0; i < 5;i++)  
  {
    int ActIndex = StartIndex + i;
    if (ActIndex <  sizeof(ConfigValue) / sizeof(ConfigValue[0]))
    {
      sprintf(buffer,"%-12.12s  %-10.10s",ConfigName[ActIndex],GetConfigValueName(ActIndex));         
      display.setCursor(0,1+i*12);      
      display.print(buffer);
      
      if (MenuSelectIndex == ActIndex)
      {      
        display.fillRect(80,i*12,128-60,9,SSD1306_INVERSE);
      }
    }
  }  
  display.display();
  delay(1);  
}

char * GetConfigValueName(int Index)
{
  switch(Index)
  {
    case CONFIG_IDX_RGB:
    return ConfigRGBValueName[ConfigValue[Index]];
    case CONFIG_IDX_PA_LEVEL:
    return ConfigPALevelName[ConfigValue[Index]];
  }
  return "";
}

void AutoCalibrate()
{  
  j1PotXCenter = analogRead(J1X_PIN);
  j1PotXRange = min((4095 - j1PotXCenter),j1PotXCenter);
  
  j1PotYCenter = analogRead(J1Y_PIN);
  j1PotYRange = min((4095 - j1PotYCenter),j1PotYCenter);

  PotCenter = analogRead(POT_PIN);
  PotRange = 800;

  Serial.println("Joystick Auto Calibrate");
  Serial.print("X: ");
  Serial.println(j1PotXCenter);
  Serial.println(j1PotXRange);
  Serial.print("Y: ");
  Serial.println(j1PotYCenter);
  Serial.println(j1PotYRange);
  Serial.print("P: ");
  Serial.println(PotCenter);
  Serial.println(PotRange);
}

int min(int a,int b)
{
  if (a < b)
    return a;
  else
    return b;
}

int max(int a,int b)
{
  if (a > b)
    return a;
  else
    return b;
}

void HandleControlInput()
{
   if (data.button1 == 1)
  {
    if (!bButton1Down)
    {
      bButton1Down = true;
      //do some btn1 staff    
     
    }      
  }
  else 
  {
    bButton1Down = false;
  }

  if (data.button2 == 1)
  {
    if (!bButton2Down)
    {
      bButton2Down = true;
      //do some btn2 staff      
     
    }      
  }
  else 
  {
    bButton2Down = false;
  }

  if (data.button3 == 1)
  {
    if (!bButton3Down)
    {
      bButton3Down = true;
      //do some btn3 staff  
      if (data.button1 == 1)
      {
         DEBUG_INFO = !DEBUG_INFO;          
      }
    }      
  }
  else 
  {
    bButton3Down = false;
  }

  if (data.button4 == 1)
  {
    if (!bButton4Down)
    {
      bButton4Down = true;
      //do some btn4 staff
      if (data.button1 == 1)
      {
        State = STATE_CONFIG;
      }
    }      
  }
  else 
  {
    bButton4Down = false;
  }          
}

void HandleConfigInput()
{
   if (data.button1 == 1)
  {
    if (!bButton1Down)
    {
      bButton1Down = true;
      //do some btn1 staff
      MenuSelectIndex++;
      MenuSelectIndex %= sizeof(ConfigValue) / sizeof(ConfigValue[0]);   
    }      
  }
  else 
  {
    bButton1Down = false;
  }

  if (data.button2 == 1)
  {
    if (!bButton2Down)
    {
      bButton2Down = true;
      //do some btn2 staff       
       MenuSelectIndex--;
      if (MenuSelectIndex < 0)
      {
        MenuSelectIndex = sizeof(ConfigValue) / sizeof(ConfigValue[0]) - 1;
      }    
    }      
  }
  else 
  {
    bButton2Down = false;
  }

  if (data.button3 == 1)
  {
    if (!bButton3Down)
    {
      bButton3Down = true;
      ConfigValue[MenuSelectIndex] = ConfigValue[MenuSelectIndex] + 1;
      if (ConfigValue[MenuSelectIndex] > ConfigValueMax[MenuSelectIndex])
      {
       ConfigValue[MenuSelectIndex] = ConfigValueMin[MenuSelectIndex];
      }      
      UpdateStripColor();
    }      
  }
  else 
  {
    bButton3Down = false;
  }

  if (data.button4 == 1)
  {
    if (!bButton4Down)
    {
      bButton4Down = true;
      //do some btn4 staff    
      WriteEEPROM();
      State = STATE_CONTROL;
    }      
  }
  else 
  {
    bButton4Down = false;
  }
}


void SetStripColor(int index,int r, int g, int b)
{
  strip.setLedColorData(index, r, g, b);     
}
void SetStripColors(int r, int g, int b)
{
  strip.setLedColorData(0, r, g, b);  
  strip.setLedColorData(1, r, g, b);  
  strip.setLedColorData(2, r, g, b);  
  strip.setLedColorData(3, r, g, b);  
}
void ShowStrip()
{
  strip.setBrightness(10);
  strip.show();
}
void UpdateStripColor()
{
  int color_index = ConfigValue[CONFIG_IDX_RGB];  
  SetStripColors(color_table[color_index][0],color_table[color_index][1],color_table[color_index][2]);
  ShowStrip();
}

void InitEEPROM()
{
  Serial.println("EEPROM Init!");
  EEPROM.begin(64);
  
  byte SaveSign = EEPROM.read(0);
  if (SaveSign != CONFIG_FLAG)
  {
    WriteEEPROM();
  }
  else
  {
    for(int i = 0; i < sizeof(ConfigName)/sizeof(ConfigName[0]);i++)
    {
      ConfigValue[i] = EEPROM.read(i+1);
    }
  }
}

void WriteEEPROM()
{
  EEPROM.write(0,CONFIG_FLAG);
  for(int i = 0; i < sizeof(ConfigName) / sizeof(ConfigName[0]);i++)
  {
     EEPROM.write(i+1,ConfigValue[i]);
  }  
  EEPROM.commit();
}
